#include "StdAfx.h"
#include ".\pillboxcollider.h"
#include <math.h>

const double lfFuzz = 0.000001;

PillboxCollider::PillboxCollider(void)
{
	// ASSERT(0);
	m_lfWidth = 1.0;
	m_p2BackPoint[0] = m_p2BackPoint[1] = m_p2FrontPoint[0] = 0.0;
	m_p2FrontPoint[1] = 1.0;

	m_lfLength = 1.0;

	
	SetRangefinders(m_lfLength, m_lfWidth, 0.3, 11);
	m_rangeFinder.ResetRanges(30.0);
	m_lfRangefinderMaxRange = 30.0;
	SetDirParams();
	SetAngleStep(0.09);
	CalcBbox();
}

PillboxCollider::PillboxCollider(double lfLength, double lfWidth, double lfXPos, double lfYPos, double lfTheta)
{
	
	double lfHalfLength = 0.5*lfLength;
	double lfSinTheta = sin(lfTheta)*lfHalfLength;
	double lfCosTheta = cos(lfTheta)*lfHalfLength;
	m_p2BackPoint[0] = lfXPos - lfCosTheta;
	m_p2BackPoint[1] = lfYPos - lfSinTheta;
	m_p2FrontPoint[0] = lfXPos + lfCosTheta;
	m_p2FrontPoint[1] = lfYPos + lfSinTheta;
	m_lfWidth = lfWidth;

	m_lfLength = lfLength;

	
	SetRangefinders(m_lfLength, m_lfWidth, 0.3, 11);
	m_rangeFinder.ResetRanges(30.0);
	m_lfRangefinderMaxRange = 30.0;
	SetDirParams();
	SetAngleStep(0.09);
	CalcBbox();
}

void PillboxCollider::SetDirParams()
{
	m_p2Dir[0] = m_p2FrontPoint[0] - m_p2BackPoint[0];
	m_p2Dir[1] = m_p2FrontPoint[1] - m_p2BackPoint[1];

	double lfDist = sqrt(m_p2Dir[0]*m_p2Dir[0] + m_p2Dir[1]*m_p2Dir[1]);
	// ASSERT(lfDist > 0.0);
	double lfScale = 1.0/lfDist;

	m_p2Dir[0] *= lfScale;
	m_p2Dir[1] *= lfScale;

	m_p2PerpDir[1] =  m_p2Dir[0];
	m_p2PerpDir[0] = -m_p2Dir[1];

	m_rangeFinder.SetXformedParams(m_p2BackPoint, m_p2Dir, m_p2PerpDir);
}

void PillboxCollider::GetGoalFromFront(const Pos2d p2Goal, Pos2d p2Out) const
{
	Pos2d p2GoalMinusCenter;
	p2GoalMinusCenter[0] = p2Goal[0]-m_p2FrontPoint[0];
	p2GoalMinusCenter[1] = p2Goal[1]-m_p2FrontPoint[1];

	p2Out[0] = m_p2Dir[0]*p2GoalMinusCenter[0] + m_p2Dir[1]*p2GoalMinusCenter[1];
	p2Out[1] = m_p2PerpDir[0]*p2GoalMinusCenter[0] + m_p2PerpDir[1]*p2GoalMinusCenter[1];
}

void PillboxCollider::SetParams(double lfLength, double lfWidth, double lfXPos, double lfYPos, double lfTheta)
{
	double lfHalfLength = 0.5*lfLength;
	double lfSinTheta = sin(lfTheta)*lfHalfLength;
	double lfCosTheta = cos(lfTheta)*lfHalfLength;
	m_p2BackPoint[0] = lfXPos - lfCosTheta;
	m_p2BackPoint[1] = lfYPos - lfSinTheta;
	m_p2FrontPoint[0] = lfXPos + lfCosTheta;
	m_p2FrontPoint[1] = lfYPos + lfSinTheta;
	m_lfWidth = lfWidth;

	m_lfLength = lfLength;

	
	SetRangefinders(m_lfLength, m_lfWidth, m_rangeFinder.GetAngleStep(), m_rangeFinder.GetNumRays()/2);
	m_rangeFinder.ResetRanges(30.0);
	m_lfRangefinderMaxRange = 30.0;
	SetDirParams();
	CalcBbox();
}

void PillboxCollider::SetPosition(double lfXPos, double lfYPos, double lfTheta)
{
	double lfHalfLength = 0.5*m_lfLength;
	double lfSinTheta = sin(lfTheta)*lfHalfLength;
	double lfCosTheta = cos(lfTheta)*lfHalfLength;
	m_p2BackPoint[0] = lfXPos - lfCosTheta;
	m_p2BackPoint[1] = lfYPos - lfSinTheta;
	m_p2FrontPoint[0] = lfXPos + lfCosTheta;
	m_p2FrontPoint[1] = lfYPos + lfSinTheta;

	SetRangefinders(m_lfLength, m_lfWidth, m_rangeFinder.GetAngleStep(), m_rangeFinder.GetNumRays()/2);
	m_rangeFinder.ResetRanges(30.0);
	m_lfRangefinderMaxRange = 30.0;
	SetDirParams();
	CalcBbox();
}

void PillboxCollider::Rotate(double lfSinTheta, double lfCosTheta, double lfPerpDist)
{
	double p2XlateAmount[2];
	p2XlateAmount[0] = lfPerpDist*m_p2PerpDir[0] - m_p2BackPoint[0];
	p2XlateAmount[1] = lfPerpDist*m_p2PerpDir[1] - m_p2BackPoint[1];
	double p2TmpVals[2];
	p2TmpVals[0] = lfCosTheta*m_p2PerpDir[0] - lfSinTheta*m_p2PerpDir[1];
	p2TmpVals[1] = lfCosTheta*m_p2PerpDir[1] + lfSinTheta*m_p2PerpDir[0];
	m_p2PerpDir[0] = p2TmpVals[0];
	m_p2PerpDir[1] = p2TmpVals[1];
	
	m_p2Dir[0] =  m_p2PerpDir[1];
	m_p2Dir[1] = -m_p2PerpDir[0];
	
	m_p2BackPoint[0] = (lfPerpDist*m_p2PerpDir[0]-p2XlateAmount[0]);
	m_p2BackPoint[1] = (lfPerpDist*m_p2PerpDir[1]-p2XlateAmount[1]);

	m_p2FrontPoint[0] = m_p2BackPoint[0]+m_lfLength*m_p2Dir[0];
	m_p2FrontPoint[1] = m_p2BackPoint[1]+m_lfLength*m_p2Dir[1];
}

PillboxCollider::~PillboxCollider(void)
{
}


void PillboxCollider::SetAngleStep(double lfAngleStep)
{
	for(int iWhichAngle = 0; iWhichAngle < 30; ++iWhichAngle)
	{
		double lfCurrAngle = lfAngleStep*(1.0+iWhichAngle);
		m_arrLfRotRadByTheta[iWhichAngle] = m_lfLength/tan(lfCurrAngle);
	}
}

void PillboxCollider::Move(int nAngleAmount, double lfDist)
{
	if(nAngleAmount == 0)
	{
		Translate(lfDist);
	}
	else 
	{
		double lfPerpDist = m_arrLfRotRadByTheta[abs(nAngleAmount)];
		// angle * radius = distance : angle = distance/radius
		double lfAngle = lfDist/lfPerpDist; // slow, consider revising
		if(nAngleAmount < 0)
		{
			Rotate(sin(lfAngle), cos(lfAngle), -lfPerpDist);
		}
		else
		{
			Rotate(-sin(lfAngle), cos(lfAngle), lfPerpDist);
		}
	}
	CalcBbox();
	m_rangeFinder.SetXformedParams(m_p2BackPoint, m_p2Dir, m_p2PerpDir);
	m_rangeFinder.ResetRanges(m_lfRangefinderMaxRange);
}


double PillboxCollider::DistanceToPoint(const double p2PointToTest[2]) const
{
	double p2DistRelBack[2];
	p2DistRelBack[0] = p2PointToTest[0] - m_p2BackPoint[0];
	p2DistRelBack[1] = p2PointToTest[1] - m_p2BackPoint[1];

	double lfDistAlongCar = p2DistRelBack[0]*m_p2Dir[0] +
		p2DistRelBack[1]*m_p2Dir[1];

	if(lfDistAlongCar < 0)
	{
		return sqrt((p2DistRelBack[0]*p2DistRelBack[0] + p2DistRelBack[1]*p2DistRelBack[1]) 
			- 0.25*m_lfWidth*m_lfWidth);
	}
	else if(lfDistAlongCar > m_lfLength)
	{
		// blatantly re-use "p2DistRelBack" as the relative distance to the front
		p2DistRelBack[0] = p2PointToTest[0] - m_p2FrontPoint[0];
		p2DistRelBack[1] = p2PointToTest[1] - m_p2FrontPoint[1];
		return sqrt((p2DistRelBack[0]*p2DistRelBack[0] + p2DistRelBack[1]*p2DistRelBack[1]) 
			- 0.25*m_lfWidth*m_lfWidth);
	}
	else
	
	return (fabs(p2DistRelBack[0]*m_p2PerpDir[0]+p2DistRelBack[1]*m_p2PerpDir[1]) - 0.5*m_lfWidth);
	
}

bool PillboxCollider::CollideSphere(const double p2PointToTest[2], double lfRadius) const
{
	double p2DistRelBack[2];
	p2DistRelBack[0] = p2PointToTest[0] - m_p2BackPoint[0];
	p2DistRelBack[1] = p2PointToTest[1] - m_p2BackPoint[1];

	double lfDistAlongCar = p2DistRelBack[0]*m_p2Dir[0] +
		p2DistRelBack[1]*m_p2Dir[1];

	if(lfDistAlongCar < 0)
	{
		return ((p2DistRelBack[0]*p2DistRelBack[0] + p2DistRelBack[1]*p2DistRelBack[1]) 
			< (0.25*m_lfWidth*m_lfWidth+lfRadius*lfRadius));
	}
	else if(lfDistAlongCar > m_lfLength)
	{
		// blatantly re-use "p2DistRelBack" as the relative distance to the front
		p2DistRelBack[0] = p2PointToTest[0] - m_p2FrontPoint[0];
		p2DistRelBack[1] = p2PointToTest[1] - m_p2FrontPoint[1];
		return ((p2DistRelBack[0]*p2DistRelBack[0] + p2DistRelBack[1]*p2DistRelBack[1]) 
			< (0.25*m_lfWidth*m_lfWidth+lfRadius*lfRadius));
	}
	else
	{
		return (fabs(p2DistRelBack[0]*m_p2PerpDir[0]+p2DistRelBack[1]*m_p2PerpDir[1]) < (0.5*m_lfWidth+lfRadius));
	}
	return false;
}


bool PillboxCollider::CollidePoint(const double p2PointToTest[2]) const
{
	double p2DistRelBack[2];
	p2DistRelBack[0] = p2PointToTest[0] - m_p2BackPoint[0];
	p2DistRelBack[1] = p2PointToTest[1] - m_p2BackPoint[1];

	double lfDistAlongCar = p2DistRelBack[0]*m_p2Dir[0] +
		p2DistRelBack[1]*m_p2Dir[1];

	if(lfDistAlongCar < 0)
	{
		return ((p2DistRelBack[0]*p2DistRelBack[0] + p2DistRelBack[1]*p2DistRelBack[1]) 
			< 0.25*m_lfWidth*m_lfWidth);
	}
	else if(lfDistAlongCar > m_lfLength)
	{
		// blatantly re-use "p2DistRelBack" as the relative distance to the front
		p2DistRelBack[0] = p2PointToTest[0] - m_p2FrontPoint[0];
		p2DistRelBack[1] = p2PointToTest[1] - m_p2FrontPoint[1];
		return ((p2DistRelBack[0]*p2DistRelBack[0] + p2DistRelBack[1]*p2DistRelBack[1]) 
			< 0.25*m_lfWidth*m_lfWidth);
	}
	else
	{
		return (fabs(p2DistRelBack[0]*m_p2PerpDir[0]+p2DistRelBack[1]*m_p2PerpDir[1]) < 0.5*m_lfWidth);
	}
	return false;
}

bool PillboxCollider::PointNearEnds(const double p2Point[2]) const
{
	double p2DistRel[2];
	p2DistRel[0] = p2Point[0] - m_p2BackPoint[0];
	p2DistRel[1] = p2Point[1] - m_p2BackPoint[1];

	int nMissesBack = ((p2DistRel[0]*p2DistRel[0] + p2DistRel[1]*p2DistRel[1]) 
			> 0.25*m_lfWidth*m_lfWidth);
	
	p2DistRel[0] = p2Point[0] - m_p2FrontPoint[0];
	p2DistRel[1] = p2Point[1] - m_p2FrontPoint[1];
	
	int nMissesFront = ((p2DistRel[0]*p2DistRel[0] + p2DistRel[1]*p2DistRel[1]) 
			> 0.25*m_lfWidth*m_lfWidth);

	return !(nMissesBack*nMissesFront);
}

bool PillboxCollider::SegmentIsectInternal(const double p2SegStart[2], const double p2SegDir[2], const double lfSegLen) const
{
	double 	p2DistRel[2];

	p2DistRel[0] = p2SegStart[0] - m_p2FrontPoint[0];
	p2DistRel[1] = p2SegStart[1] - m_p2FrontPoint[1];

	double lfLenToPerp = p2DistRel[0]*p2SegDir[0]+p2DistRel[1]*p2SegDir[1];
	if(lfLenToPerp <= 0.0 && lfLenToPerp >= -lfSegLen)
	{
		if(((p2DistRel[0]*p2DistRel[0]+p2DistRel[1]*p2DistRel[1]-lfLenToPerp*lfLenToPerp)
			< 0.25*m_lfWidth*m_lfWidth))
		{
			return true;
		}
	}


	p2DistRel[0] = p2SegStart[0] - m_p2BackPoint[0];
	p2DistRel[1] = p2SegStart[1] - m_p2BackPoint[1];

	lfLenToPerp = p2DistRel[0]*p2SegDir[0]+p2DistRel[1]*p2SegDir[1];
	if(lfLenToPerp <= 0.0 && lfLenToPerp >= -lfSegLen)
	{
		if(((p2DistRel[0]*p2DistRel[0]+p2DistRel[1]*p2DistRel[1]-lfLenToPerp*lfLenToPerp)
			< 0.25*m_lfWidth*m_lfWidth))
		{
			return true;
		}
	}

	double lfParrDistStart = p2DistRel[0]*m_p2Dir[0]+    p2DistRel[1]*m_p2Dir[1];
	double lfPerpDistStart = p2DistRel[0]*m_p2PerpDir[0]+p2DistRel[1]*m_p2PerpDir[1];
	double lfDirCompParr =   p2SegDir[0]* m_p2Dir[0]+    p2SegDir[1]* m_p2Dir[1];
	double lfDirCompPerp =   p2SegDir[0]* m_p2PerpDir[0]+p2SegDir[1]* m_p2PerpDir[1];

	double lfParrDistToEnd = lfDirCompParr*lfSegLen;

	if(fabs(lfParrDistStart) > fabs(lfParrDistToEnd))
	{
		return false;
	}

	if(lfParrDistStart < lfFuzz)
	{
		if(lfDirCompParr < lfFuzz) // implicitly checks of lfDirCompParr is not equal to zero
		{
			return false;
		}

		
		lfPerpDistStart -= lfParrDistStart*lfDirCompPerp/lfDirCompParr; 
		// good thing lfDirCompParr is not equal to zero!!

		lfParrDistToEnd += lfParrDistStart; // because lfParrDistStart is negative
		lfParrDistStart = 0.0; // do we need to compute this?
	}

	if(lfParrDistStart > m_lfLength)
	{
		if(lfDirCompParr > -lfFuzz) // implicitly checks of lfDirCompParr is not equal to zero
		{
			return false;
		}
		lfPerpDistStart += (m_lfLength-lfParrDistStart)*lfDirCompPerp/lfDirCompParr; 
		// good thing lfDirCompParr is not equal to zero!!

		lfParrDistToEnd += (m_lfLength - lfParrDistStart);
		lfParrDistStart = m_lfLength; // do we need to compute this?
	}

	double lfOvershoot = lfParrDistToEnd+lfParrDistStart-m_lfLength;
	if(lfOvershoot > -lfFuzz)
	{
		lfParrDistToEnd -= lfOvershoot;
	}

	lfOvershoot = m_lfLength+lfOvershoot;
	if(lfOvershoot < lfFuzz)
	{
		lfParrDistToEnd -= lfOvershoot;
	}
	
	double lfPerpDistAtEnd;
	
	if(fabs(lfDirCompParr) > lfFuzz)
	{
		lfPerpDistAtEnd = lfPerpDistStart + lfDirCompPerp*lfParrDistToEnd/lfDirCompParr;
	}
	else
	{
		lfPerpDistAtEnd = lfPerpDistStart + lfSegLen*lfDirCompPerp;
	}
	
	
	
	return (fabs(lfPerpDistStart) < 0.5*m_lfWidth) || 
		(fabs(lfPerpDistAtEnd) < 0.5*m_lfWidth) || 
		(lfPerpDistAtEnd*lfPerpDistStart < 0.0);
}

bool PillboxCollider::CollideLine(const double p2PointLineStart[2], const double p2PointLineEnd[2]) const
{
	if(PointNearEnds(p2PointLineStart))
		return true;
	if(PointNearEnds(p2PointLineEnd))
		return true;
	double p2SegDir[2];
	p2SegDir[0] = p2PointLineEnd[0]-p2PointLineStart[0];
	p2SegDir[1] = p2PointLineEnd[1]-p2PointLineStart[1];

	double lfSegLen = sqrt(p2SegDir[0]*p2SegDir[0]+p2SegDir[1]*p2SegDir[1]);

	if(lfSegLen < lfFuzz)
	{
		return false;
	}
	double lfScale = 1.0/lfSegLen;
	p2SegDir[0] *= lfScale;
	p2SegDir[1] *= lfScale;

	return SegmentIsectInternal(p2PointLineStart, p2SegDir, lfSegLen);
}

void PillboxCollider::GetDrawparams(double p2BackCenter[], double p2FrontCenter[], double p2DirNormed[]) const
{
	p2BackCenter[0]  = m_p2BackPoint[0] ; p2BackCenter[1]  = m_p2BackPoint[1];
	p2FrontCenter[0] = m_p2FrontPoint[0]; p2FrontCenter[1] = m_p2FrontPoint[1];
	p2DirNormed[0] = m_p2Dir[0]; p2DirNormed[1] = m_p2Dir[1];
}

void PillboxCollider::CalcBbox()
{
	m_arrLfBbox[2] = m_arrLfBbox[0] = m_p2FrontPoint[0];
	m_arrLfBbox[3] = m_arrLfBbox[1] = m_p2FrontPoint[1];
	for(int nAxis = 0; nAxis < 2; ++nAxis)
	{
		if(m_arrLfBbox[nAxis] > m_p2BackPoint[nAxis])
		{
			m_arrLfBbox[nAxis] = m_p2BackPoint[nAxis];
		}
		if(m_arrLfBbox[nAxis+2] < m_p2BackPoint[nAxis])
		{
			m_arrLfBbox[nAxis+2] = m_p2BackPoint[nAxis];
		}
		m_arrLfBbox[nAxis] -= 0.5*m_lfWidth;
		m_arrLfBbox[nAxis+2] += 0.5*m_lfWidth;
	}
}

bool PillboxCollider::CollideConvexPoly(const double arrP2PolyBoundary[][2], int nNumPts) const
{
	for(int iWhichPoint = 0; iWhichPoint < nNumPts; ++iWhichPoint)
	{
		if(PointNearEnds(arrP2PolyBoundary[iWhichPoint]))
		{
			PointNearEnds(arrP2PolyBoundary[iWhichPoint]);
			return true;
		}
	}

	for(int iWhichPoint = 0; iWhichPoint < nNumPts; ++iWhichPoint)
	{
		int iNextPoint = (iWhichPoint+1)%nNumPts;
		double p2SegDir[2];
		p2SegDir[0] = arrP2PolyBoundary[iNextPoint][0]-arrP2PolyBoundary[iWhichPoint][0];
		p2SegDir[1] = arrP2PolyBoundary[iNextPoint][1]-arrP2PolyBoundary[iWhichPoint][1];

		double lfSegLen = sqrt(p2SegDir[0]*p2SegDir[0]+p2SegDir[1]*p2SegDir[1]);

		if(lfSegLen < lfFuzz)
		{
			continue;
		}
		double lfScale = 1.0/lfSegLen;
		p2SegDir[0] *= lfScale;
		p2SegDir[1] *= lfScale;

		if(SegmentIsectInternal(arrP2PolyBoundary[iWhichPoint], p2SegDir, lfSegLen))
		{
			SegmentIsectInternal(arrP2PolyBoundary[iWhichPoint], p2SegDir, lfSegLen);
			return true;
		}
	}
	return false;
}
